import cv2
import numpy as np
import glob
import time
from concurrent.futures import ProcessPoolExecutor, ThreadPoolExecutor
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

w = 53   #标定板中圆点个数
h = 53

# 世界坐标系中的棋盘格点,例如(0,0,0), (1,0,0), (2,0,0) ....,(8,5,0)，去掉Z坐标，记为二维矩阵
objp = np.zeros((w*h,3), np.float32)
objp[:,:2] = np.mgrid[0:w,0:h].T.reshape(-1,2)
# 储存棋盘格角点的世界坐标和图像坐标对
objpoints = [] # 在世界坐标系中的三维点
imgpoints = [] # 在图像平面的二维点


def calc(fname):
    print("start calc",fname)
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
    params = cv2.SimpleBlobDetector_Params()
    params.maxArea = 10e4
    params.minArea = 5
    params.minDistBetweenBlobs = 1
    blobDetector = cv2.SimpleBlobDetector_create(params)
    ret, corners = cv2.findCirclesGrid(gray, (w, h), cv2.CALIB_CB_SYMMETRIC_GRID, blobDetector, None)
    if ret == True:
        cv2.drawChessboardCorners(img, (w,h), corners, ret)
        cv2.imwrite(fname+'_suc.png',img)
        print("success",fname)
        return corners
    else:#识别失败
        print("fail",fname)
        return []
if __name__=="__main__":
    fimages = glob.glob('imgR/*.jpg')#右侧相机拍的
    #fimages = glob.glob('imgL/*.jpg')
    pool = ProcessPoolExecutor(max_workers=61)
    results=pool.map(calc,fimages)

    for i in results:
        if len(i)!=0:
            objpoints.append(objp)
            imgpoints.append(i)
    cv2.destroyAllWindows()
    #标定、去畸变
    gray=cv2.imread(fimages[0])
    gray = cv2.cvtColor(gray,cv2.COLOR_BGR2GRAY)
    ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)
    #ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None,flags=cv2.CALIB_ZERO_TANGENT_DIST)
    # mtx：内参数矩阵
    # dist：畸变系数
    # rvecs：旋转向量 （外参数）
    # tvecs ：平移向量 （外参数）
    print (("ret:"),ret)
    print (("mtx:\n"),mtx)
    print (("dist:\n"),dist)
    print (("rvecs:\n"),rvecs)
    print (("tvecs:\n"),tvecs)
    # 去畸变
    # 计算出的畸变参数很大，反畸变后似乎出锅了
    #img2 = cv2.imread(r"C:\Users\yanqi\Desktop\rbgame\img415_3\L19.jpg")
    #h,w = img2.shape[:2]
    #newcameramtx, roi=cv2.getOptimalNewCameraMatrix(mtx,dist,(w,h),0,(w,h))
    #dst = cv2.undistort(img2, mtx, dist, None, newcameramtx)
    #x,y,w,h = roi
    #dst = dst[y:y+h, x:x+w]
    #cv2.imwrite('calibresult.jpg',dst)

    # 反投影误差
    total_error = 0
    for i in range(len(objpoints)):
        imgpoints2, _ = cv2.projectPoints(objpoints[i], rvecs[i], tvecs[i], mtx, dist)
        error = cv2.norm(imgpoints[i],imgpoints2, cv2.NORM_L2)/len(imgpoints2)
        print(error)
        total_error += error
    print (("total error: "), total_error/len(objpoints))


